#include "stm32f10x.h"                  // Device header
#include "Motor.h"
#include "Encoder.h"
#include "PWM.h"
#include "PID.h"
#include "OLED.h"
#include "Serial.h"

extern PID_t LeftPID[];
extern PID_t RightPID[];

// 根据设备对应更改，中心角度值不准确会导致摆杆总是有往一个方向跑的趋势，位置环PD控制器有稳态误差，最终按键控制横杆正转一圈和反转一圈的速度不同
#define CENTER_ANGLE		0.5			// 中心角度值                               
#define CENTER_RANGE		50			// 中心区间范围

int16_t Speed1, Location1, Speed2, Location2;	// 左右电机的速度和位置

void BalanceCar_Init(void)
{
	Encoder_Init();
	Motor_Init();
}

void BalanceCar_Read(void)
{
	Speed1 = Encoder_GetCount(ENCODER_A);	// 获取电机的速度值
	Location1 += Speed1;					// 获取电机的位置值
	Speed2 = Encoder_GetCount(ENCODER_B);
	Location2 += Speed2;
}

void BalanceCar_InnerPID(float Date)
{
	/*以下进行角度环PID控制*/
	LeftPID[0].Actual = Date;			// 内环为角度环，实际值为角度值
	PID_Update(&LeftPID[0]);			// 调用封装好的函数，一步完成PID计算和更新
	Motor_SetPWM(0, LeftPID[0].Out);	// 角度环的输出值给到电机PWM
	
	RightPID[0].Actual = Date;			// 内环为角度环，实际值为角度值
	PID_Update(&RightPID[0]);			// 调用封装好的函数，一步完成PID计算和更新
	Motor_SetPWM(1, RightPID[0].Out);	// 角度环的输出值给到电机PWM
	
//	OLED_Printf(64, 0, OLED_6X8, "lo1:%+05d", Location1);
//	OLED_Printf(64, 8, OLED_6X8, "lo2:%+05d", Location2);
	
//	OLED_Printf(64, 0, OLED_6X8, "Tar1:%+05d", LeftPID[0].Target);
//	OLED_Printf(64, 8, OLED_6X8, "Act1:%+05d", LeftPID[0].Actual);
//	OLED_Printf(64, 16, OLED_6X8, "Out1:%+05d", LeftPID[0].Out);
//	
//	OLED_Printf(64, 24, OLED_6X8, "Tar2:%+05d", RightPID[0].Target);
//	OLED_Printf(64, 32, OLED_6X8, "Act2:%+05d", RightPID[0].Actual);
//	OLED_Printf(64, 40, OLED_6X8, "Out2:%+05d", RightPID[0].Out);
	
//	Serial_Printf("%f, %f, %f\r\n",LeftPID[0].Target, LeftPID[0].Actual, LeftPID[0].Out);
}

void BalanceCar_OuterPID(void)
{
	/*以下进行位置环PID控制*/
	LeftPID[1].Actual = Location1;						// 外环为位置环，实际值为位置值
	PID_Update(&LeftPID[1]);							// 调用封装好的函数，一步完成PID计算和更新
	LeftPID[0].Target = CENTER_ANGLE - LeftPID[1].Out;	// 外环的输出值作用于内环的目标值，组成串级PID结构
	
	RightPID[1].Actual = Location2;						// 外环为位置环，实际值为位置值
	PID_Update(&RightPID[1]);							// 调用封装好的函数，一步完成PID计算和更新
	RightPID[0].Target = CENTER_ANGLE + RightPID[1].Out;// 外环的输出值作用于内环的目标值，组成串级PID结构
	
	OLED_Printf(64, 16, OLED_6X8, "lp1:%+05.0f", LeftPID[1].Out);
	OLED_Printf(64, 24, OLED_6X8, "rp2:%+05.0f", RightPID[1].Out);
}

